﻿// Inner Fire 游戏引擎库
// JointConstraint - 穿透约束
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 所属层级：应用层（4）
// 用于解决物体之间的穿透。头文件在constraint.h。此CPP用于分装，没有头文件
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-06-30
#include "constraint.h"
#include <game/physics/rigid_body.h>

namespace ifire::game {

PenetrationConstraint::PenetrationConstraint()
    : Constraint(), friction(0.0f), normal(0.0f, 0.0f, 0.0f) {}

PenetrationConstraint::PenetrationConstraint(
    RigidBody* a, RigidBody* b, IVec3 a_point, IVec3 b_point, IVec3 normal)
    : Constraint(), friction(0.0f) {
  this->a = a;
  this->b = b;
  this->aPoint = a->WorldToLocal(a_point);
  this->bPoint = b->WorldToLocal(b_point);
  this->normal = normal;
  cached_lambda.Zero();
}

void PenetrationConstraint::PreSolve(float dt) {
  // 1. 获取世界空间碰撞点和法线（确保normal方向从A指向B）
  auto a_point = a->LocalToWorld(aPoint);
  auto b_point = b->LocalToWorld(bPoint);
  auto pa = XMLoadFloat3(&a_point);
  auto pb = XMLoadFloat3(&b_point);
  // normal始终是A指向B
  auto normal_vec = XMVector3Normalize(XMLoadFloat3(&normal));

  // 2. 计算冲量臂（从质心到接触点）
  auto ra_vec = pa - XMLoadFloat3(&a->position_);
  auto rb_vec = pb - XMLoadFloat3(&b->position_);

  // 3. 初始化Jacobian矩阵（2行：法向+切向摩擦）
  jacobian.Zero();

  // ===== 法向约束（第0行） =====
  // 物体A的线性部分（-n）
  jacobian[0][0] = -normal.x;
  jacobian[0][1] = -normal.y;
  jacobian[0][2] = -normal.z;

  // 物体A的角部分 (-rA × n)
  const IVec3 angularA = StoreF3(-XMVector3Cross(ra_vec, normal_vec));
  jacobian[0][3] = angularA.x;
  jacobian[0][4] = angularA.y;
  jacobian[0][5] = angularA.z;

  // 物体B的线性部分（+n）
  jacobian[0][6] = normal.x;
  jacobian[0][7] = normal.y;
  jacobian[0][8] = normal.z;

  // 物体B的角部分 (+rB × n)
  const IVec3 angularB = StoreF3(XMVector3Cross(rb_vec, normal_vec));
  jacobian[0][9] = angularB.x;
  jacobian[0][10] = angularB.y;
  jacobian[0][11] = angularB.z;

  // 静态物体处理
  if (b->inv_mass_ <= 0.0f) {
    jacobian[0][6] = jacobian[0][7] = jacobian[0][8] = 0.0f;
    jacobian[0][9] = jacobian[0][10] = jacobian[0][11] = 0.0f;
  }

  // ===== 计算Baumgarte稳定项 =====
  auto a_angular = XMLoadFloat3(&a->angular_velocity_);
  auto b_angular = XMLoadFloat3(&b->angular_velocity_);
  va = XMLoadFloat3(&a->velocity_) + XMVector3Cross(a_angular, ra_vec);
  vb = XMLoadFloat3(&b->velocity_) + XMVector3Cross(b_angular, rb_vec);
  vrel_dot_n = XMVectorGetX(XMVector3Dot(va - vb, normal_vec));
  const float e = (std::min)(a->restitution_, b->restitution_);

  // ===== 切向摩擦约束（第1行） =====
  friction = (std::max)(a->friction_, b->friction_);
  if (friction > 0.0f) {
    // 计算基于相对速度的切向方向
    XMVECTOR tangent_vec = va - vb;
    tangent_vec = XMVector3Normalize(
        tangent_vec - normal_vec * XMVector3Dot(tangent_vec, normal_vec));
    if (XMVectorGetX(XMVector3LengthSq(tangent_vec)) < 0.1f) {
      tangent_vec = XMVector3Normalize(
          XMVector3Cross(normal_vec, XMVectorSet(0, 1, 0, 0)));
    }
    auto tangent = StoreF3(tangent_vec);

    // 物体A的线性部分（-t）
    jacobian[1][0] = -tangent.x;
    jacobian[1][1] = -tangent.y;
    jacobian[1][2] = -tangent.z;

    // 物体A的角部分 (-rA × t)
    const IVec3 friction_angular_A =
        StoreF3(-XMVector3Cross(ra_vec, tangent_vec));
    jacobian[1][3] = friction_angular_A.x;
    jacobian[1][4] = friction_angular_A.y;
    jacobian[1][5] = friction_angular_A.z;

    // 物体B的线性部分（+t）
    jacobian[1][6] = tangent.x;
    jacobian[1][7] = tangent.y;
    jacobian[1][8] = tangent.z;

    // 物体B的角部分 (+rB × t)
    const IVec3 friction_angular_B =
        StoreF3(XMVector3Cross(rb_vec, tangent_vec));
    jacobian[1][9] = friction_angular_B.x;
    jacobian[1][10] = friction_angular_B.y;
    jacobian[1][11] = friction_angular_B.z;
  }

  // ===== 预热启动 =====
  const MatMN Jt = jacobian.Transpose();
  VecN impulses = Jt * cached_lambda;
  a->ApplyImpulseLinear(IVec3(impulses[0], impulses[1], impulses[2]));
  a->ApplyImpulseAngular(IVec3(impulses[3], impulses[4], impulses[5]));
  b->ApplyImpulseLinear(IVec3(impulses[6], impulses[7], impulses[8]));
  b->ApplyImpulseAngular(IVec3(impulses[9], impulses[10], impulses[11]));

  // 关键修改：穿透深度计算与2D一致（使用 -normal_vec）
  const float depth = XMVectorGetX(XMVector3Dot(pb - pa, -normal_vec));
  const float slop = 0.001f;
  const float C = (std::min)(0.0f, depth + slop); // 当穿透时C < 0

  // Baumgarte项（beta和符号与2D一致）
  const float beta = 0.2f;
  bias = (beta / dt) * C + e * vrel_dot_n;
}

void PenetrationConstraint::Solve() {
  const auto V = GetVelocities();
  const auto inv_m = GetInvM();

  auto& J = jacobian;
  const auto Jt = jacobian.Transpose();

  // 计算分子
  auto lhs = J * inv_m * Jt; // A
  auto rhs = J * V * -1.0f;  // b
  rhs[0] -= bias;

  // 使用Gaus-Seidel求解Ax=b
  auto lambda = MatMN<2, 2>::SolveGaussSeidel(lhs, rhs);
  VecN<2> old_lambda = cached_lambda;
  cached_lambda += lambda;
  cached_lambda[0] = (cached_lambda[0] < 0.0f) ? 0.0f : cached_lambda[0];

  // 限制摩擦冲量
  if (friction > 0.0f) {
    float max_friction = cached_lambda[0] * friction;
    cached_lambda[1] =
        std::clamp(cached_lambda[1], -max_friction, max_friction);

    // 可选：根据速度动态减少摩擦
    auto normal_vec = XMVector3Normalize(XMLoadFloat3(&normal));
    float tangent_speed =
        XMVectorGetX(XMVector3Length(va - vb - normal_vec * vrel_dot_n));
    if (tangent_speed > 1.0f) {
      cached_lambda[1] *= 0.5f;
    }
  }
  lambda = cached_lambda - old_lambda;

  // 物体的冲量
  auto impulses = Jt * lambda;
  a->ApplyImpulseLinear(IVec3(impulses[0], impulses[1], impulses[2]));
  a->ApplyImpulseAngular(IVec3(impulses[3], impulses[4], impulses[5]));
  b->ApplyImpulseLinear(IVec3(impulses[6], impulses[7], impulses[8]));
  b->ApplyImpulseAngular(IVec3(impulses[9], impulses[10], impulses[11]));
}

void PenetrationConstraint::PostSolve() {}

} // namespace ifire::game